#pragma once
#include <vector>
#include <list>
#include <memory>
#include <map>

struct ZucPose;
class AxisGroupPose;
namespace axisgroup {

class LPF1Filter;
class NLFFilter;
class MMFFilter;

class ServoFilterIntf
{
public:
    ServoFilterIntf();
    void init_servo_filter(int joint_num, AxisGroupPose* pose, const double* vel_limit, const double* acc_limit, double time);
    void do_cart_filt(double* positions);
    void do_cart_filt(ZucPose& positions);
    void do_joint_filter(double* positions);

    enum FilterType
    {
        NONE = 0,
        LPF_1ST = 1,
        NLF_JOINT = 2,
        MMF_JOINT = 3,
        NLF_CART = 4,
        NLF_MMF_COMB = 5,
    };
    FilterType get_filter_type() const { return servo_filter_type_; }
    std::string get_filter_name();
    void set_filter_type(FilterType type) { servo_filter_type_ = type; };  // #TODO, 真的处于serov模式时，不允许切换滤波类型
    void set_lpf_para(double lpf_cutoff_freq);
    void set_nlf_para(double nlf_vp, double nlf_ap, double nlf_jp, double nlf_vr, double nlf_ar, double nlf_jr);
    void set_mmf_para(int mmf_buf_size, double mmf_ka, double mmf_kv, double mmf_kp);

private:
    // 滤波器关节维数
    size_t joint_num_ = 0;

    //滤波器参数
    FilterType servo_filter_type_ = FilterType::NONE;
    static std::map<FilterType, std::string> FILTER_TYPE_NAME;

    std::shared_ptr<LPF1Filter> lpf_filter_;
    std::shared_ptr<NLFFilter> nlf_filter_;
    std::shared_ptr<MMFFilter> mmf_filter_;
};

/*****************************************************
 * NLF 滤波器
******************************************************/
class NLFFilter
{
public:
    struct NLFInfo
    {
        double target_pos;
        double cur_pos;
        double cur_vel;
        double cur_acc;
        double cur_jerk;
        double max_vel;
        double max_acc;
        double max_jerk;
        double Ts;
    } nlf_data[7];

    int nlf_init(int joint, double pos);
    int nlf_set_bounds(int joint, double vel, double acc, double jerk, double Ts);
    int nlf_set_target(int joint, double pos);
    int nlf_c3(int joint);
    double u_a(double a, double dde_k);
    double delta_v(double v, double dde_k, double de_k);
    double u_cv(double v, double dde_k, double de_k);
    double u_v(double v, double dde_k, double dde_min, double dde_max, double de_k);

    double U;
    double nlf_vp_;
    double nlf_ap_;
    double nlf_jp_;
    double nlf_vr_;
    double nlf_ar_;
    double nlf_jr_;
};

/*****************************************************
 * 低通 滤波器
******************************************************/
class LPF1Filter
{
public:
    struct LPF1stInfo
    {
        double curr_pos;
        double tar_pos;
        double cutoff_freq;
    } lpf1st_data[7];

    int lpf_init(int joint, double pos, double fs);
    int lpf_set_target(int joint, double pos);
    int lpf_process(int joint);

    double fs_ = 0.008;  // 采样时间
};

/*****************************************************
 * MMF 滤波器
******************************************************/
class MMFFilter
{
public:
    int mmf_buf_size_;
    double mmf_ka_ = 0;
    double mmf_kv_ = 0;
    double mmf_kp_ = 0;
    std::vector<std::vector<double>> filter_buffer_data;
    void mmf_init(int size, std::vector<double> pos);
    void mmf_init(int size, const double* pos);
    void push_buffer_data(double cmd, int joint_num);
    double mmf_process(int joint_num);
    double mmf_process_cart(int axis_num);

private:
    double target_pos_[7];
    double target_vel_[7];
    double target_acc_[7];
    double target_jrk_[7];
    double target_pos_pre_[7];

    double sum_buffer_data(int start, int end, int joint_num);
};
}  // namespace axisgroup